/****************************************Copyright (c)****************************************************

**--------------File Info---------------------------------------------------------------------------------
** File name:                pid.c
** Last modified Date:       2024.11.16
** Last Version:             Ver:1.0
** Descriptions:             PID控制驱动
**
**--------------------------------------------------------------------------------------------------------
** Created by:               adm
** Created date:             2024.11.15
** Version:                  Ver:1.0
** Descriptions:
**--------------------------------------------------------------------------------------------------------*/
#include "pid.h"


PID mypid = {0}; //创建一个PID结构体变量


/**
  * @brief    初始化pid参数函数
  * @param    None
  * @retval   None
  */
void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
{
    pid->kp = p;                //比例系数
    pid->ki = i;                //积分系数
    pid->kd = d;                //微分系数
    pid->maxIntegral = maxI;    //积分限幅
    pid->maxOutput = maxOut;    //输出限幅
}


/**
  * @brief    进行一次pid计算
  *           参数为(pid结构体,目标值,反馈值)，计算结果放在pid结构体的output成员中
  * @param    reference:目标值   feedback:测量值
  * @retval   None
  */
void PID_Calc(PID *pid, float reference, float feedback)
{
    //更新数据
    pid->lastError = pid->error; //将旧error存起来
    pid->error = reference - feedback; //计算新error
    //计算微分
    float dout = (pid->error - pid->lastError) * pid->kd;
    //计算比例
    float pout = pid->error * pid->kp;
    //计算积分
    pid->integral += pid->error * pid->ki;

    //积分限幅
    if(pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral;

    //计算输出
    pid->output = pout + dout + pid->integral;

    //输出限幅
    if(pid->output > pid->maxOutput) pid->output =   pid->maxOutput;
    else if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput;
}

